/******************************************************************************
**
** File Name:	rtos_library.S
**
** Description:	RTOS Kernel Library code. Internal RTOS services implementing
**				critical manipulations with basic RTOS object - null-terminated 
**				linked list.
**
** RTOS ver:	3.0
**
** Modules/Functions:
**
**		_EnqueueObj		( RTOS_OBJ_t * pid )
**		_AppendObj		( RTOS_OBJ_t * pid )
**		_RemoveObj		( RTOS_OBJ_t * pid )
**		_RemoveObjTop	( RTOS_OBJ_t * pid )
**		_RemoveObjEx	( RTOS_OBJ_t * pid )
**	
** Notes:	See description below prior to use RTOS Kernel Library services!
**
*******************************************************************************
** History:
**
**	10/03/2011 - AK - Separating RTOS linked list manipulation functions into 
**					the	dedicated library module.
**	11/20/2011 - AK - Kernel library modified to opearte with pid (RTOS_OBJ_t *)
**					as input parameter 
**	07/26/2012 - AK - RTOS API is downgraded to comply with earliest rtos v2
**  08/08/2012 - AK - RTOS source code is checked for byte-by-byte complience 
**					with MPP binary (Dec 8, 2011).
**	09/20/2012 - AK - RTOS v3. _ENTER_CRITICAL/_LEAVE_CRITICAL 
**								 _RTOS_PROLOG/_RTOS_EPILOG
** 		
******************************************************************************/
#include        "rtos.inc"

        _MODULE(rtos_library)
        _CODESECTION

/******************************************************************************
** Prototype:	void _EnqueueObj( RTOS_OBJ_t * pid )
** Description:	Takes Object ID (process ID) and inserts it into 
**				the run queue.  
**				The run queue is sorted by priority. Lower number - highest 
**				priority. If there are multiple tasks of equal priority, then 
**				the new task is attached to the list of equals (round robin).
** Passed:		p1h:p1l = object ID to enqueue
** Returns:		r1l:	= -1  - Suspended
**						= 0   - Top of run queue
**						= 1-N - Depth in run queue
** Uses:		Y, Z, tmp0-3, SREG, RunQueue
** Calls:		
** Assumes:		
** Notes:		RTOS Internal Service. Not to use in C-code!!!
******************************************************************************/
    _FUNCTION(_EnqueueObj)

_EnqueueObj:                  
            
    mov     Zl,   p1l								;; Zl:Zh = TaskID to insert (InsertID)
    mov     Zh,   p1h
    ldi		tmp1, lo8(-1)

    ldd     tmp0, Z+PidState      					;; tmp0 = Flags
    andi    tmp0, (BV(SuspendBit) | BV(IdleBit)) 	;; if marked Suspended or Idle
	
    brne    _eo02									;; then go to SUSPEND

	push	Yl										;; Save Y to stack
	push	Yh		

    ldd     tmp2, Z+PidPriority						;; tmp2 = InsertID->Priority
    ldi     Yl, lo8(RtosKernelData+RunQueue)		;; Yl:Yh = top of Run queue
    ldi     Yh, hi8(RtosKernelData+RunQueue)
	in		tmp0, _SFR_IO_ADDR(SREG)				;; tmp0 = SREG
	
;;	cli												;; not allowed for RTOS v2.
	
_eo00:												;; looking for the place in the list to insert 

	inc		tmp1     								;; tmp1 = counter of insertion point.
    mov     Zl, Yl         							;; Zl:Zh = CurrentID (starting from head of run queue).
    mov     Zh, Yh
    ldd     Yl, Z+PidNext+NextL						;; Z = CurrentID, Y = NextID
    ldd     Yh, Z+PidNext+NextH     				
    adiw    Yl, 0									;; if NextID == 0 (end of run queue)
	
    breq    _eo01           						;; Then end of queue, continue
	
    ldd     tmp3, Y+PidPriority						;; tmp3 = NextID->Priority
    cp      tmp2, tmp3								;; if InsertID > CurrentID
	
    brsh    _eo00           						;; Loop until InsertID > CurrentID 
	
_eo01:												;; actually insert the ID into the place found on previous step

    std     Z+NextH, p1h							;; CurrentID->Next = InsertID
    std     Z+NextL, p1l    						
    mov     Zh, p1h									;; Zl:Zh = InsertID
    mov     Zl, p1l
    std     Z+NextH, Yh     						;; InsertID->Next = Next
    std     Z+NextL, Yl
	pop		Yh										;; Restore Y from Stack
	pop		Yl		
    mov		r1l, tmp1								;; r1l = place inside the list where the ID had been iserted
	out		_SFR_IO_ADDR(SREG), tmp0				;; restore SREG
	
	ret												;; return
	
_eo02:												;; Suspend

	mov		r1l, tmp1								;; r1l = -1 (return Suspended)
    sbr     tmp0, BV(SuspendedBit)  				;; Mark suspended and return
    std     Z+PidState, tmp0						;; TaskID->flags = SUSPENDED
	
	ret			

    _ENDFUNC	;; _FUNCTION(_EnqueueObj)

/******************************************************************************
** Prototype:	void _AppendObj(RTOS_OBJ_t * pid)
** Description:	Append object ID specified to the end of the queue (null 
**				terminated list).
** Passed:		Z		= head of objects queue
**				p2h:p2l	= pointer to ObjID to append to the queue				
** Returns:		Z		= ObjID that had been inserted
**				Flags
** Uses:		Z, Flags, tmp0, tmp1
** Calls:		
** Assumes:		
** Notes:		RTOS Internal Service. No C function prototype.
**				Not to use in C-code!!!
******************************************************************************/
_ao00:

	mov     Zl, tmp0				;; Zh:Zl = ObjID->NextID
    mov     Zh, tmp1

    _FUNCTION(_AppendObj)

_AppendObj:							;; starts with Zh:Zl = Top of the queue

    ldd     tmp0, Z+NextL			;; tmp1:tmp0 = ObjID->NextID
    ldd     tmp1, Z+NextH			
    subi    tmp0, lo8(0)			;; if ObjID != 0 (end of the queue)
    sbci    tmp1, hi8(0)			
    
	brne    _ao00					;; loop until end of the queue

    std     Z+NextH, p2h			;; ObjID->NextID = ObjID - append the ObjID 
    std     Z+NextL, p2l			
    mov     Zh, p2h					;; Zh:Zl = TaskID
    mov     Zl, p2l
    std     Z+NextH, tmp1			;; TaskID->NextID = 0 (now this is end of queue)
    std     Z+NextL, tmp0

    ret								;; return

    _ENDFUNC	;; _FUNCTION(_AppendObj)

/******************************************************************************
** Prototype:	_RemoveObj(RTOS_OBJ_t * pid)
** Description:	Removes object ID from the task queue (null-terminated list).  
**				Returns 0 if failed.
** Passed:		Z         = Head of the queue
**              p2h:p2l   = Object ID
** Returns:		p2h:p2l   = Object ID
**              tmp1:tmp0 = Object or 0
**              Z         = Next object in list
**              Z Flag set if Failed
**              Z Flag cleared if success
** Uses:		Z, Flags
** Calls:		_RemoveObjEx
** Assumes:		Must be called with interrupts disabled
** Notes:		RTOS Internal Service. Not to use in C-code!!!
******************************************************************************/
_ro00:

	mov     Zh, tmp1				;; CurrentID = CurrentID->Next
    mov     Zl, tmp0

    _FUNCTION(_RemoveObj)

_RemoveObj:							;; Entering this point with Zh:Zl = CurrentID (Head of the queue)

    ldd     tmp0, Z+NextL			;; tmp1:tmp0 = CurrentID->Next
    ldd     tmp1, Z+NextH			
    cp      p2l, tmp0				;; if CurrentID->Next == ObjID
    cpc     p2h, tmp1

    breq    _RemoveObjEx			;; Then goto remove service 
									;; p2h:p2l = Object ID		removing object
									;; Zh:Zl   = Previous ID	removing object predessor
									
    subi    tmp0, lo8(0)			;; Check if CurrentID->Next is the end of list
    sbci    tmp1, hi8(0)			
	
    brne    _ro00					;; If not then continue

    ret								;; End of list

    _ENDFUNC	;; _FUNCTION(_RemoveObj)

/******************************************************************************
** Prototype:	_RemoveObjTop(RTOS_OBJ_t * pid)
** Description:	Removes the first (top) item from the task queue 
**				(null-terminated list). Return zero if empty.
** Passed:		Z       = Head of the queue
** Returns:		p2h:p2l = 0 if list empty, otherwize
**              Z       = Next Item
**              Zero Flag set if P2 == 0
**              Zero Flag cleared if P2 != 0
** Uses:		Flags
** Calls:		_RemoveObjEx
** Assumes:		Must be called with interrupts disabled
** Notes:		RTOS Internal Service. Not to use in C-code!!!
******************************************************************************/

    _FUNCTION(_RemoveObjTop)

_RemoveObjTop:

    ldd     p2l, Z+NextL		;; p2h:p2l = TopID->NextID
    ldd     p2h, Z+NextH     	;; 
    subi    p2l, lo8(0)			;; Check if next object does not exist (only top in the queue)
    sbci    p2h, hi8(0)      	

    brne    _RemoveObjEx		;; 
								;; p2h:p2l = Object ID		removing object
								;; Zh:Zl   = Previous ID	removing object predessor

    ret

    _ENDFUNC	;; _FUNCTION(_RemoveObjTop)

/******************************************************************************
** Prototype:	_RemoveObjEx(RTOS_OBJ_t * pid)
** Description:	Remove specified item from the task queue (null-terminated 
**				list). 
** Passed:		Z       = PrevID - ID of the object that is previous to one 
**						  that will be removed
**              p2h:p2l = ObjID  - ID that will be removed
** Returns:		p2h:p2l = ObjID  - removed ID
**              Z       = NextID 
**              Zero flag cleared
** Uses:		Y,Z,tmp2,tmp3
** Calls:		None
** Assumes:		Must be called with interrupts disabled
** Notes:		RTOS Internal Service. Not to use in C-code!!!
******************************************************************************/

	_FUNCTION(_RemoveObjEx)

_RemoveObjEx:					

    mov     tmp2, Yl				;; tmp3:tmp2 = Yh:Yl
    mov     tmp3, Yh
    mov     Yl, p2l					;; Yh:Yl = ObjID
    mov     Yh, p2h

    ldd     p2h, Y+NextH			;; p2h:p2l = ObjID->NextID
    ldd     p2l, Y+NextL
    std     Z+NextH, p2h			;; TopID->NextID = ObjID->NextID
    std     Z+NextL, p2l			;; 
    mov     Zl, p2l					;; Return NextID in Z
    mov     Zh, p2h
    clr     p2l
    std     Y+NextL, p2l			;; ObjID->NextID = 0
    std     Y+NextH, p2l

    mov     p2h, Yh					;; restore p2 & Y
    mov     p2l, Yl
    mov     Yl, tmp2
    mov     Yh, tmp3
	
    clz								;; Return non-zero

    ret

    _ENDFUNC	;; _FUNCTION(_RemoveObjEx)


    _END		;; _MODULE(rtos_library)

		
		